#include "MMatrix4.hpp"


//! Constructor - sets identity matrix.
MMatrix4::MMatrix4()
{
	SetIdentity();
}

//! Constructor - argument is MMAtrix4 reference.
MMatrix4::MMatrix4( const MMatrix4& m )
{
	m_matrix[0]  = m.m_matrix[0];  
	m_matrix[4]  = m.m_matrix[4];  
	m_matrix[8]  = m.m_matrix[8];  
	m_matrix[12] = m.m_matrix[12]; 

	m_matrix[1]  = m.m_matrix[1];  
	m_matrix[5]  = m.m_matrix[5];  
	m_matrix[9]  = m.m_matrix[9];  
	m_matrix[13] = m.m_matrix[13]; 

	m_matrix[2]  = m.m_matrix[2];  
	m_matrix[6]  = m.m_matrix[6];  
	m_matrix[10] = m.m_matrix[10]; 
	m_matrix[14] = m.m_matrix[14]; 

	m_matrix[3]  = m.m_matrix[3];
	m_matrix[7]  = m.m_matrix[7];
	m_matrix[11] = m.m_matrix[11];
	m_matrix[15] = m.m_matrix[15];
}

//! Constructor - arguments are values for each matrix's cell.
MMatrix4::MMatrix4( float f0,  float f1,  float f2,  float f3,
					float f4,  float f5,  float f6,  float f7,
					float f8,  float f9,  float f10, float f11,
					float f12, float f13, float f14, float f15 )
{
    m_matrix[0]  = f0;  m_matrix[1]  = f1;  m_matrix[2]  = f2;  m_matrix[3]  = f3;
	m_matrix[4]  = f4;  m_matrix[5]  = f5;  m_matrix[6]  = f6;  m_matrix[7]  = f7;
	m_matrix[8]  = f8;  m_matrix[9]  = f9;  m_matrix[10] = f10; m_matrix[11] = f11;
	m_matrix[12] = f12; m_matrix[13] = f13; m_matrix[14] = f14; m_matrix[15] = f15;
}

//! Destructor.
MMatrix4::~MMatrix4()
{
}

//! Sets identity matrix.
void MMatrix4::SetIdentity()
{
	//! Set identity matrix
    m_matrix[0]  = 1.0f; m_matrix[1]  = 0.0f; m_matrix[2]  = 0.0f; m_matrix[3]  = 0.0f;
	m_matrix[4]  = 0.0f; m_matrix[5]  = 1.0f; m_matrix[6]  = 0.0f; m_matrix[7]  = 0.0f;
	m_matrix[8]  = 0.0f; m_matrix[9]  = 0.0f; m_matrix[10] = 1.0f; m_matrix[11] = 0.0f;
	m_matrix[12] = 0.0f; m_matrix[13] = 0.0f; m_matrix[14] = 0.0f; m_matrix[15] = 1.0f;
}

//! Sets all cells to zero.
void MMatrix4::SetZeros()
{
	for( int i = 0; i < 16; ++i )
		m_matrix[i] = 0;
}

//! Overloaded assignment operator.
MMatrix4& MMatrix4::operator =(const MMatrix4 &m)
{
	if( this != &m )
	{
		m_matrix[0]  = m.m_matrix[0];  
		m_matrix[4]  = m.m_matrix[4];  
		m_matrix[8]  = m.m_matrix[8];  
		m_matrix[12] = m.m_matrix[12]; 

		m_matrix[1]  = m.m_matrix[1];  
		m_matrix[5]  = m.m_matrix[5];  
		m_matrix[9]  = m.m_matrix[9];  
		m_matrix[13] = m.m_matrix[13]; 

		m_matrix[2]  = m.m_matrix[2];  
		m_matrix[6]  = m.m_matrix[6];  
		m_matrix[10] = m.m_matrix[10]; 
		m_matrix[14] = m.m_matrix[14]; 

		m_matrix[3]  = m.m_matrix[3];
		m_matrix[7]  = m.m_matrix[7];
		m_matrix[11] = m.m_matrix[11];
		m_matrix[15] = m.m_matrix[15];
	}

	return *this;
}

//! Overloaded product operator.
MMatrix4 MMatrix4::operator *(MMatrix4 &m)
{
	return MMatrix4( 
		//! Row: 1
		//! Element: matrix[0]
		m_matrix[0]*m.m_matrix[0] + m_matrix[1]*m.m_matrix[4] + m_matrix[2]*m.m_matrix[8] + m_matrix[3]*m.m_matrix[12],
		//! Element: matrix[1]
		m_matrix[0]*m.m_matrix[1] + m_matrix[1]*m.m_matrix[5] + m_matrix[2]*m.m_matrix[9] + m_matrix[3]*m.m_matrix[13],
		//! Element: matrix[2]
		m_matrix[0]*m.m_matrix[2] + m_matrix[1]*m.m_matrix[6] + m_matrix[2]*m.m_matrix[10] + m_matrix[3]*m.m_matrix[14],
		//! Element: matrix[3]
		m_matrix[0]*m.m_matrix[3] + m_matrix[1]*m.m_matrix[7] + m_matrix[2]*m.m_matrix[11] + m_matrix[3]*m.m_matrix[15],

		//! Row: 2
		//! Element: matrix[4]
		m_matrix[4]*m.m_matrix[0] + m_matrix[5]*m.m_matrix[4] + m_matrix[6]*m.m_matrix[8] + m_matrix[7]*m.m_matrix[12],
		//! Element: matrix[5]
		m_matrix[4]*m.m_matrix[1] + m_matrix[5]*m.m_matrix[5] + m_matrix[6]*m.m_matrix[9] + m_matrix[7]*m.m_matrix[13],
		//! Element: matrix[6]
		m_matrix[4]*m.m_matrix[2] + m_matrix[5]*m.m_matrix[6] + m_matrix[6]*m.m_matrix[10] + m_matrix[7]*m.m_matrix[14],
		//! Element: matrix[7]
		m_matrix[4]*m.m_matrix[3] + m_matrix[5]*m.m_matrix[7] + m_matrix[6]*m.m_matrix[11] + m_matrix[7]*m.m_matrix[15],

		//! Row: 3
		//! Element: matrix[8]
		m_matrix[8]*m.m_matrix[0] + m_matrix[9]*m.m_matrix[4] + m_matrix[10]*m.m_matrix[8] + m_matrix[11]*m.m_matrix[12],
		//! Element: matrix[9]
		m_matrix[8]*m.m_matrix[1] + m_matrix[9]*m.m_matrix[5] + m_matrix[10]*m.m_matrix[9] + m_matrix[11]*m.m_matrix[13],
		//! Element: matrix[10]
		m_matrix[8]*m.m_matrix[2] + m_matrix[9]*m.m_matrix[6] + m_matrix[10]*m.m_matrix[10] + m_matrix[11]*m.m_matrix[14],
		//! Element: matrix[11]
		m_matrix[8]*m.m_matrix[3] + m_matrix[9]*m.m_matrix[7] + m_matrix[10]*m.m_matrix[11] + m_matrix[11]*m.m_matrix[15],

		//! Row: 4
		//! Element: matrix[12]
		m_matrix[12]*m.m_matrix[0] + m_matrix[13]*m.m_matrix[4] + m_matrix[14]*m.m_matrix[8] + m_matrix[15]*m.m_matrix[12],
		//! Element: matrix[13]
		m_matrix[12]*m.m_matrix[1] + m_matrix[13]*m.m_matrix[5] + m_matrix[14]*m.m_matrix[9] + m_matrix[15]*m.m_matrix[13],
		//! Element: matrix[14]
		m_matrix[12]*m.m_matrix[2] + m_matrix[13]*m.m_matrix[6] + m_matrix[14]*m.m_matrix[10] + m_matrix[15]*m.m_matrix[14],
		//! Element: matrix[15]
		m_matrix[12]*m.m_matrix[3] + m_matrix[13]*m.m_matrix[7] + m_matrix[14]*m.m_matrix[11] + m_matrix[15]*m.m_matrix[15]
		);
}

//! Overloaded product operator.
MMatrix4 MMatrix4::operator *(float multiplier)
{
	return MMatrix4( m_matrix[0]*multiplier,  m_matrix[1]*multiplier,  m_matrix[2]*multiplier,  m_matrix[3]*multiplier,
					 m_matrix[4]*multiplier,  m_matrix[5]*multiplier,  m_matrix[6]*multiplier,  m_matrix[7]*multiplier, 
					 m_matrix[8]*multiplier,  m_matrix[9]*multiplier,  m_matrix[10]*multiplier, m_matrix[11]*multiplier,
					 m_matrix[12]*multiplier, m_matrix[13]*multiplier, m_matrix[14]*multiplier, m_matrix[15]*multiplier );
}

//! Overloaded addition operator.
MMatrix4 MMatrix4::operator +(MMatrix4& m)
{
	return MMatrix4( m_matrix[0] + m.m_matrix[0], m_matrix[1] + m.m_matrix[1], m_matrix[2] + m.m_matrix[2],
		             m_matrix[3] + m.m_matrix[3], m_matrix[4] + m.m_matrix[4], m_matrix[5] + m.m_matrix[5],
					 m_matrix[6] + m.m_matrix[6], m_matrix[7] + m.m_matrix[7], m_matrix[8] + m.m_matrix[8],
					 m_matrix[9] + m.m_matrix[9], m_matrix[10] + m.m_matrix[10], m_matrix[11] + m.m_matrix[11],
					 m_matrix[12] + m.m_matrix[12], m_matrix[13] + m.m_matrix[13], m_matrix[14] + m.m_matrix[14],
					 m_matrix[15] + m.m_matrix[15] );
}

//! Overloaded subtract operator.
MMatrix4 MMatrix4::operator -(MMatrix4& m)
{
	return MMatrix4( m_matrix[0] - m.m_matrix[0], m_matrix[1] - m.m_matrix[1], m_matrix[2] - m.m_matrix[2],
		             m_matrix[3] - m.m_matrix[3], m_matrix[4] - m.m_matrix[4], m_matrix[5] - m.m_matrix[5],
					 m_matrix[6] - m.m_matrix[6], m_matrix[7] - m.m_matrix[7], m_matrix[8] - m.m_matrix[8],
					 m_matrix[9] - m.m_matrix[9], m_matrix[10] - m.m_matrix[10], m_matrix[11] - m.m_matrix[11],
					 m_matrix[12] - m.m_matrix[12], m_matrix[13] - m.m_matrix[13], m_matrix[14] - m.m_matrix[14],
					 m_matrix[15] - m.m_matrix[15] );
}

bool MMatrix4::InverseMatrix( MMatrix4 m )
{
// Copyright January 2004
// All Rights Reserved!
// Allen Sherrod
// ProgrammingAce@UltimateGameProgramming.com
// www.UltimateGameProgramming.com
//
// TEMPORARY BORROWED

   float tempMatrix[16] = {0};
   float d12, d13, d23, d24, d34, d41;

	d12 = m.m_matrix[2]  * m.m_matrix[7]  - m.m_matrix[3]  * m.m_matrix[6];
	d13 = m.m_matrix[2]  * m.m_matrix[11] - m.m_matrix[3]  * m.m_matrix[10];
	d23 = m.m_matrix[6]  * m.m_matrix[11] - m.m_matrix[7]  * m.m_matrix[10];
	d24 = m.m_matrix[6]  * m.m_matrix[15] - m.m_matrix[7]  * m.m_matrix[14];
	d34 = m.m_matrix[10] * m.m_matrix[15] - m.m_matrix[11] * m.m_matrix[14];
	d41 = m.m_matrix[14] * m.m_matrix[3]  - m.m_matrix[15] * m.m_matrix[2];

	tempMatrix[0] =   m.m_matrix[5] * d34 - m.m_matrix[9] * d24 + m.m_matrix[13] * d23;
	tempMatrix[1] = -(m.m_matrix[1] * d34 + m.m_matrix[9] * d41 + m.m_matrix[13] * d13);
	tempMatrix[2] =   m.m_matrix[1] * d24 + m.m_matrix[5] * d41 + m.m_matrix[13] * d12;
	tempMatrix[3] = -(m.m_matrix[1] * d23 - m.m_matrix[5] * d13 + m.m_matrix[9]  * d12);

	// Calculate the determinant.
	float determinant = m.m_matrix[0] * tempMatrix[0] + m.m_matrix[4] * tempMatrix[1] +
                       m.m_matrix[8] * tempMatrix[2] + m.m_matrix[12] * tempMatrix[3];

	// Clear if the determinant is equal to zero.  0 means matrix have no inverse.
	if(determinant == 0.0)
	   {
		   SetIdentity();
         return false;
	   }

	float invDeterminant = 1.0f / determinant;
	
	// Compute rest of inverse.
	tempMatrix[0] *= invDeterminant;
	tempMatrix[1] *= invDeterminant;
	tempMatrix[2] *= invDeterminant;
	tempMatrix[3] *= invDeterminant;

	tempMatrix[4] = -(m.m_matrix[4] * d34 - m.m_matrix[8] * d24 + m.m_matrix[12] * d23) * invDeterminant;
	tempMatrix[5] =   m.m_matrix[0] * d34 + m.m_matrix[8] * d41 + m.m_matrix[12] * d13  * invDeterminant;
	tempMatrix[6] = -(m.m_matrix[0] * d24 + m.m_matrix[4] * d41 + m.m_matrix[12] * d12) * invDeterminant;
	tempMatrix[7] =   m.m_matrix[0] * d23 - m.m_matrix[4] * d13 + m.m_matrix[8]  * d12  * invDeterminant;

	// Pre-compute 2x2 dets for first two rows when computing cofactors 
	// of last two rows.
	d12 = m.m_matrix[0]  * m.m_matrix[5]  - m.m_matrix[1]  * m.m_matrix[12];
	d13 = m.m_matrix[0]  * m.m_matrix[9]  - m.m_matrix[1]  * m.m_matrix[8];
	d23 = m.m_matrix[4]  * m.m_matrix[9]  - m.m_matrix[5]  * m.m_matrix[8];
	d24 = m.m_matrix[4]  * m.m_matrix[13] - m.m_matrix[5]  * m.m_matrix[12];
	d34 = m.m_matrix[8]  * m.m_matrix[13] - m.m_matrix[9]  * m.m_matrix[12];
	d41 = m.m_matrix[12] * m.m_matrix[1]  - m.m_matrix[13] * m.m_matrix[0];

	tempMatrix[8]  =   m.m_matrix[7] * d34 - m.m_matrix[11] * d24 + m.m_matrix[15] * d23 * invDeterminant;
	tempMatrix[9]  = -(m.m_matrix[3] * d34 + m.m_matrix[11] * d41 + m.m_matrix[15] * d13) * invDeterminant;
	tempMatrix[10] =   m.m_matrix[3] * d24 + m.m_matrix[7]  * d41 + m.m_matrix[15] * d12 * invDeterminant;
	tempMatrix[11] = -(m.m_matrix[3] * d23 - m.m_matrix[7]  * d13 + m.m_matrix[11] * d12) * invDeterminant;
	tempMatrix[12] = -(m.m_matrix[6] * d34 - m.m_matrix[10] * d24 + m.m_matrix[14] * d23) * invDeterminant;
	tempMatrix[13] =   m.m_matrix[2] * d34 + m.m_matrix[10] * d41 + m.m_matrix[14] * d13 * invDeterminant;
	tempMatrix[14] = -(m.m_matrix[2] * d24 + m.m_matrix[6]  * d41 + m.m_matrix[14] * d12) * invDeterminant;
	tempMatrix[15] =   m.m_matrix[2] * d23 - m.m_matrix[6]  * d13 + m.m_matrix[10] * d12 * invDeterminant;

   // Save the temp matrix to our matrix.
   m_matrix[0]  = tempMatrix[0];  m_matrix[1]  = tempMatrix[1];
   m_matrix[2]  = tempMatrix[2];  m_matrix[3]  = tempMatrix[3];
	m_matrix[4]  = tempMatrix[4];  m_matrix[5]  = tempMatrix[5];
   m_matrix[6]  = tempMatrix[6];  m_matrix[7]  = tempMatrix[7];
	m_matrix[8]  = tempMatrix[8];  m_matrix[9]  = tempMatrix[9];
   m_matrix[10] = tempMatrix[10]; m_matrix[11] = tempMatrix[11];
	m_matrix[12] = tempMatrix[12]; m_matrix[13] = tempMatrix[13];
   m_matrix[14] = tempMatrix[14]; m_matrix[15] = tempMatrix[15];

   return true;
}

void MMatrix4::TransposeMatrix()
{
	MMatrix4 tempMatrix = *this;

	//m_matrix[0] = tempMatrix.m_matrix[0];
	m_matrix[1] = tempMatrix.m_matrix[4];
	m_matrix[2] = tempMatrix.m_matrix[8];
	m_matrix[3] = tempMatrix.m_matrix[12];

	m_matrix[4] = tempMatrix.m_matrix[1];
	//m_matrix[5] = tempMatrix.m_matrix[5];
	m_matrix[6] = tempMatrix.m_matrix[9];
	m_matrix[7] = tempMatrix.m_matrix[13];

	m_matrix[8] = tempMatrix.m_matrix[2];
	m_matrix[9] = tempMatrix.m_matrix[6];
	//m_matrix[10] = tempMatrix.m_matrix[10];
	m_matrix[11] = tempMatrix.m_matrix[14];

	m_matrix[12] = tempMatrix.m_matrix[3];
	m_matrix[13] = tempMatrix.m_matrix[7];
	m_matrix[14] = tempMatrix.m_matrix[11];
	//m_matrix[15] = tempMatrix.m_matrix[15];
}